#include <boost/thread.hpp>
#include <unordered_set>
#include <path_planning/offline_processor.h>

typedef std::list<Cell*> List;
typedef std::unordered_set<Cell*> CellSet;

namespace {
// Default values
const int kPlayerPort = 6665;
const int kMapProxyInstance = 1;
const string kHostName = "localhost";

const int kCellEmpty = -1;
const int kCellUnknown = 0;
const int kCellOccupied = 1;

const int kNoVertex = -1;

inline bool ContainsCell(CellSet cell_set, Cell *cell) {
  return cell_set.find(cell) != cell_set.end();
}

void RunSearch(GridMap *grid_map, const std::vector<PixelPoint> &vertices,
        int offset, int thread_step, std::list<Cell*> *undefined_cells);

void ResolveUndefinedCells(GridMap *grid_map, std::list<Cell*> **undefined_cells,
        int num_threads);
}

GridMap::GridMap(MapProxy *mapproxy) {
  map_width_ = mapproxy->GetWidth();
  map_height_ = mapproxy->GetHeight();
  map_resolution_ = mapproxy->GetResolution();

  cell_matrix_ = new Cell*[map_width_ * map_height_];

  for(uint32_t i = 0; i < map_height_; ++i)
    for (uint32_t j = 0; j < map_width_; ++j) {
      Cell* c = new Cell;
      c->status = mapproxy->GetCell(j, i);
      c->column = j;
      c->row = i;
      c->vertex_id = kNoVertex;
      c->associated_vertex_id = kNoVertex;
      cell_matrix_[i * map_width_ + j] = c;
    }
}

GridMap::~GridMap() {
  for (uint32_t i = 0; i < map_width_ * map_height_; ++i)
    delete cell_matrix_[i];
  delete[] cell_matrix_;
}

Cell* GridMap::GetCell(uint32_t column, uint32_t row) {
  return (column >= 0 && column < map_width_ && row >= 0 && row < map_height_) ?
          cell_matrix_[row * map_width_ + column] : NULL;
}

inline uint32_t GridMap::map_width() const {
  return map_width_;
}

inline uint32_t GridMap::map_height() const {
  return map_height_;
}

inline double GridMap::map_resolution() const {
  return map_resolution_;
}

OfflineProcessor::OfflineProcessor(int num_threads) :
        num_threads_(num_threads), grid_map_(NULL), topological_map_(NULL) {
}

void OfflineProcessor::Init(string host_name = kHostName, int player_port_num = kPlayerPort,
        int map_proxy_instance = kMapProxyInstance) {
  PlayerClient robot_client(host_name, player_port_num);
  MapProxy mapproxy(&robot_client, map_proxy_instance);
  mapproxy.RequestMap();

  grid_map_ = new GridMap(&mapproxy);
}

void OfflineProcessor::SetTopologicalMap(string topological_map_file_descriptor) {
  if (topological_map_)
    delete topological_map_;

  topological_map_ = new PlannerMap();
  topological_map_->SetGraphDescriptor(topological_map_file_descriptor);

  for (int i = 0; i < topological_map_->Size(); ++i) {
    Vertex *vertex = topological_map_->GetVertex(i);
    PixelPoint *point = RealCoordToPixel(vertex->x_coordinate, vertex->y_coordinate,
            grid_map_->map_resolution(), grid_map_->map_width(), grid_map_->map_height());
    Cell *cell = grid_map_->GetCell(point->x, point->y);
    cell->vertex_id = vertex->id;
    vertices_.push_back(*point);

    delete point;
  }
}

void OfflineProcessor::Run(string grid_map_path_file) {
  using boost::thread;
  using std::list;

  cout << "Time: " << time(0) << endl;
  cout << "OfflineProcessor::Processing Gridmap..." << endl;

  thread **threads = new thread*[num_threads_];
  list<Cell*> **undefined_cells = new list<Cell*>*[num_threads_];

  for (int i = 0; i < num_threads_; ++i) {
    undefined_cells[i] = new list<Cell*>();
    threads[i] = new boost::thread(RunSearch, grid_map_, vertices_,
            i, num_threads_, undefined_cells[i]);
  }
  for (int i = 0; i < num_threads_; ++i)
    threads[i]->join();

  ResolveUndefinedCells(grid_map_, undefined_cells, num_threads_);

  for (int i = 0; i < num_threads_; ++i) {
    delete threads[i];
    delete undefined_cells[i];
  }
  delete threads;
  delete undefined_cells;

  FILE *output = fopen(grid_map_path_file.c_str(), "w");

  fprintf(output, "%f ", grid_map_->map_resolution());
  fprintf(output, "%d ", grid_map_->map_width());
  fprintf(output, "%d \n", grid_map_->map_height());

  for (uint32_t i = 0; i < grid_map_->map_height(); ++i) {
    for (uint32_t j = 0; j < grid_map_->map_width(); ++j)
      fprintf(output, "%2d ", grid_map_->GetCell(j, i)->associated_vertex_id);
    fprintf(output, "\n");
  }

  fclose(output);

  cout << "Map generated at " << grid_map_path_file << "." << endl;
  cout << "Time: " << time(0) << endl;
}

void OfflineProcessor::Clear() {
  delete grid_map_;
  grid_map_ = NULL;

  delete topological_map_;
  topological_map_ = NULL;

  vertices_.clear();
}

namespace {
bool FastSearch(GridMap *grid_map, const std::vector<PixelPoint> &vertices,
        Cell *cell) {
  int min_distance = grid_map->map_height() + grid_map->map_width();
  const PixelPoint *vertexPoint;

  int distance;
  for (std::vector<PixelPoint>::const_iterator it = vertices.begin();
          it != vertices.end(); ++it) {
    distance = abs(it->y - cell->row) + abs(it->x - cell->column);
    if (distance < min_distance) {
      min_distance = distance;
      vertexPoint = &*it;
    }
  }
  Cell *vertex = grid_map->GetCell(vertexPoint->x, vertexPoint->y);

  // -------------- [Horizontal --> Vertical] Search --------------
  int i = cell->row, j = cell->column;
  bool hvSearch = true;
  while (hvSearch && j < vertex->column) {
    if (grid_map->GetCell(++j, i)->status != kCellEmpty)
      hvSearch = false;
  }
  while (hvSearch && j > vertex->column) {
    if (grid_map->GetCell(--j, i)->status != kCellEmpty)
      hvSearch = false;
  }
  while (hvSearch && i < vertex->row) {
    if (grid_map->GetCell(j, ++i)->status != kCellEmpty)
      hvSearch = false;
  }
  while (hvSearch && i > vertex->row) {
    if (grid_map->GetCell(j, --i)->status != kCellEmpty)
      hvSearch = false;
  }
  if (hvSearch) {
    cell->associated_vertex_id = vertex->vertex_id;
    return true;
  }

  // -------------- [Vertical --> Horizontal] Search --------------
  i = cell->row, j = cell->column;
  bool vhSearch = true;
  while (vhSearch && i < vertex->row) {
    if (grid_map->GetCell(j, ++i)->status != kCellEmpty)
      vhSearch = false;
  }
  while (vhSearch && i > vertex->row) {
    if (grid_map->GetCell(j, --i)->status != kCellEmpty)
      vhSearch = false;
  }
  while (vhSearch && j < vertex->column) {
    if (grid_map->GetCell(++j, i)->status != kCellEmpty)
      vhSearch = false;
  }
  while (vhSearch && j > vertex->column) {
    if (grid_map->GetCell(--j, i)->status != kCellEmpty)
      vhSearch = false;
  }
  if (vhSearch) {
    cell->associated_vertex_id = vertex->vertex_id;
    return true;
  }

  return false;
}

void FullSearch(GridMap *grid_map, Cell *cell) {
  CellSet cell_set;
  cell_set.insert(cell);
  List pending_list;
  pending_list.push_back(cell);

  while (!pending_list.empty()) {
    Cell *current_cell = pending_list.front();
    pending_list.pop_front();

    for (int m = -1; m < 2; m++) {
      for (int n = -1; n < 2; n++) {
        if (m == n || m == -n)
          continue;

        uint32_t adjacent_column = n + current_cell->column;
        uint32_t adjacent_row = m + current_cell->row;

        if ((adjacent_column < 0) || (adjacent_row < 0) ||
                (adjacent_column > grid_map->map_width() - 1) ||
                (adjacent_row > grid_map->map_height() - 1))
          continue;

        Cell *adjacent_cell = grid_map->GetCell(adjacent_column, adjacent_row);

        if (adjacent_cell->status != kCellOccupied && !ContainsCell(cell_set, adjacent_cell)) {
          if (adjacent_cell->vertex_id != kNoVertex) {
            cell->associated_vertex_id = adjacent_cell->vertex_id;
            return;
          }
          cell_set.insert(adjacent_cell);
          pending_list.push_back(adjacent_cell);
        }
      }
    }
  }
}

void RunSearch(GridMap *grid_map, const std::vector<PixelPoint> &vertices,
        int offset, int thread_step, std::list<Cell*> *undefined_cells) {
  for (uint32_t i = 0; i < grid_map->map_height(); ++i) {
    for (uint32_t j = offset; j < grid_map->map_width(); j += thread_step) {
      Cell *cell = grid_map->GetCell(j, i);

      if (cell->vertex_id != kNoVertex) {
        cell->associated_vertex_id = cell->vertex_id;
        continue;
      }

      if (cell->status != kCellEmpty)
        continue;

      if (!FastSearch(grid_map, vertices, cell))
        undefined_cells->push_back(cell);
    }
  }
}

void ResolveUndefinedCells(GridMap *grid_map, std::list<Cell*> **undefined_cells,
        int num_threads) {
  int num_undefined_cells = 0;
  for (int i = 0; i < num_threads; ++i)
    num_undefined_cells += undefined_cells[i]->size();
  printf("Undefined cells: %d\n", num_undefined_cells);

  while (num_undefined_cells != 0) {
    for (int i = 0; i < num_threads; ++i) {
      std::list<Cell*> *current_list = undefined_cells[i];

      std::list<Cell*>::iterator it = current_list->begin();
      while (it != current_list->end()) {
        Cell *cell = *it;

        Cell *neighbour = grid_map->GetCell(cell->column + 1, cell->row);
        if (neighbour && neighbour->associated_vertex_id != kNoVertex) {
          cell->associated_vertex_id = neighbour->associated_vertex_id;
          it = current_list->erase(it);
          --num_undefined_cells;
          continue;
        }

        neighbour = grid_map->GetCell(cell->column, cell->row + 1);
        if (neighbour && neighbour->associated_vertex_id != kNoVertex) {
          cell->associated_vertex_id = neighbour->associated_vertex_id;
          it = current_list->erase(it);
          --num_undefined_cells;
          continue;
        }

        neighbour = grid_map->GetCell(cell->column - 1, cell->row);
        if (neighbour && neighbour->associated_vertex_id != kNoVertex) {
          cell->associated_vertex_id = neighbour->associated_vertex_id;
          it = current_list->erase(it);
          --num_undefined_cells;
          continue;
        }

        neighbour = grid_map->GetCell(cell->column, cell->row - 1);
        if (neighbour && neighbour->associated_vertex_id != kNoVertex) {
          cell->associated_vertex_id = neighbour->associated_vertex_id;
          it = current_list->erase(it);
          --num_undefined_cells;
          continue;
        }

        ++it;
      }
    }
  }
}
}
